//
// Created by ze on 2023/12/30.
//

#include "UPRE_ROBOT_TASK.h"
#include "UPRE_PATH.h"
#include "UPRE_PID.h"
#include "UPRE_MOVEBASE.h"
#include "UPRE_LOCATION.h"
#include "cmsis_os.h"
#include "UPRE_Hand_Shank.h"
#include "UPRE_AirCylinder.h"
#include "UPRE_GYRO100A.h"
#include "UPRE_Decision.h"
//
// Created by ze on 2023/11/10.
//
uint8_t chassis[26]  ={0};
uint8_t g_Auto_Num = 0;
static uint8_t Only_Once = 0;
//直线路径任务
static StraightLinePath line = { 0 };

void GoBesselTask1(void)
{
    static PATH_BesselControlPoint Control_Point;
    static Pose gopath3 = { -2585.53f, 2306.34f,-89.73f};  //直线路径3
    static Pose gopath6 = { (-2118.6f), (2059.3f),-270.8f};  //直线路径6
    static Pose gopath9 = { (-1211.25f), (2218.85f),-449.6f};  //直线路径9
    static Pose gopath12 = { (-718.26f), (1973.39f),-630.f};  //直线路径12
    static Pose gopath13 = { (642.f ), (2044.14f),-630.f};  //直线路径13     //平移到坡前
    static Pose gopath14 = { (725.78f), (5554.29f),-632.f};  //开始上坡
    switch (g_Auto_Num)
    {
        /////路径一开始          //第一次取苗
        case 0:
            if(Pack_receive.is_start_chassic)
            {
            Control_Point.Path_BesselPoint_1 = Robot_Pose;

            Control_Point.Path_BesselPoint_2.x = -405.f + PATH_FIX_X;
            Control_Point.Path_BesselPoint_2.y = 423.f + PATH_FIX_Y;

            Control_Point.Path_BesselPoint_3.x = -3355.f;
            Control_Point.Path_BesselPoint_3.y = 1300.f;

            Control_Point.Path_BesselPoint_4.wx = -3355.f;
            Control_Point.Path_BesselPoint_4.wy = 190.f;
            Control_Point.Path_BesselPoint_4.yaw = 0.f;

            g_Auto_Num++;
            }
            break;
        case 1:

            if(Path_BesselPathInit(&Control_Point, false, 3500, 3500, 400, 5, 40, 80));
            chassis[10]=1;
            g_Auto_Num++;

            break;
        case 2:
            if (goPath() == FINISH) {
                setPidCalMode(PID_OFF);

                g_Auto_Num++;
            }
            break;
            /////路径一结束
        case 3:

            /****右取苗开始****/

            chassis[0]=1;

            /****右取苗结束****/
//            g_Auto_Num++;
//            Motor_ClearPID();
            break;
            /////路径二开始                                  //第一次放苗
        case 4:

            Control_Point.Path_BesselPoint_1 = Robot_Pose;
            Control_Point.Path_BesselPoint_2.x = -3212.f;//-3355+143
            Control_Point.Path_BesselPoint_2.y = 1017.f;//190+827

            Control_Point.Path_BesselPoint_3.x = -3183.f;//-3061-122
            Control_Point.Path_BesselPoint_3.y = 1566.f;//2070.22f -504.22

            Control_Point.Path_BesselPoint_4.wx = -3046.33f;
            Control_Point.Path_BesselPoint_4.wy = 2080.22f;
            Control_Point.Path_BesselPoint_4.yaw = -90.28f;
            g_Auto_Num++;

            break;
        case 5:
            if(Path_BesselPathInit(&Control_Point, false, 2000, 2500, 400, 5, 40, 70));

            g_Auto_Num++;

            break;
        case 6:
            if (goPath() == FINISH) {
                setPidCalMode(PID_OFF);

                g_Auto_Num++;
            }
            break;
            /////路径二结束
        case 7:
//            HAL_GPIO_TogglePin(LED2_GPIO_Port,LED2_Pin);
            /****右放苗2开始****/

            chassis[2]=1;

            /****右取苗2结束****/
//            g_Auto_Num++;
            break;
            /////路径三开始                         //第二次放苗
        case 8:
            line.startPose = Robot_Pose;
            line.endPose = gopath3;
            line.endPose.yaw = optimizeRobRotation(line.endPose.yaw);

            g_Auto_Num++;

            break;
        case 9:
            if(StraigthLinePathInit(&line, false, 1500, 1500, 400, 10, 60, 20));

            g_Auto_Num++;

            break;
        case 10:
            if (goPath() == FINISH) {

//                setPidCalMode(PID_OFF);
                g_Auto_Num++;
            }
            break;
            /////路径三结束
        case 11:
            /****右放苗1开始****/

            chassis[3]=1;

            /****右放苗1结束****/
//            g_Auto_Num++;
//            Motor_ClearPID();
            break;
            /////路径四开始              //第二次取苗
        case 12:

            Control_Point.Path_BesselPoint_1 = Robot_Pose;

            Control_Point.Path_BesselPoint_2.x = -2021.93f;
            Control_Point.Path_BesselPoint_2.y = 2209.04f;

            Control_Point.Path_BesselPoint_3.x = -2770.18f;
            Control_Point.Path_BesselPoint_3.y = 887.1f;

            Control_Point.Path_BesselPoint_4.wx = -2677.f;
            Control_Point.Path_BesselPoint_4.wy = 174.1f;
            Control_Point.Path_BesselPoint_4.yaw = -180.25f;

            g_Auto_Num++;

            break;
        case 13:
            if(Path_BesselPathInit(&Control_Point, false, 3000, 3000, 400, 5, 50, 60));

            g_Auto_Num++;

            break;
        case 14:
            if (goPath() == FINISH) {

                setPidCalMode(PID_OFF);
                g_Auto_Num++;
            }
            break;
            /////路径四结束
        case 15:
            /****左取苗开始****/

            chassis[1]=1;

            /****左取苗结束****/
//            g_Auto_Num++;
            break;
            /////路径五开始                  //第三次放苗
        case 16:
            Control_Point.Path_BesselPoint_1 = Robot_Pose;

            Control_Point.Path_BesselPoint_2.x = -2534.f;
            Control_Point.Path_BesselPoint_2.y = 1005.1f;

            Control_Point.Path_BesselPoint_3.x = -2710.1f;
            Control_Point.Path_BesselPoint_3.y = 1352.88f;

            Control_Point.Path_BesselPoint_4.wx = -2578.1f;
            Control_Point.Path_BesselPoint_4.wy = 1852.1f;
            Control_Point.Path_BesselPoint_4.yaw = -268.f;

            g_Auto_Num++;

            break;
        case 17:
            if(Path_BesselPathInit(&Control_Point, false, 2000, 2500, 400, 5, 40, 70));
            g_Auto_Num++;

            break;
        case 18:
            if (goPath() == FINISH) {

                setPidCalMode(PID_OFF);
                g_Auto_Num++;
            }
            break;
            /////路径五结束
        case 19:
            /****左放苗1开始****/

            chassis[4]=1;

            /****左放苗1结束****/
//            g_Auto_Num++;
//            Motor_ClearPID();
            break;
            /////路径六开始                  //第四次放苗
        case 20:

            line.startPose = Robot_Pose;
            line.endPose = gopath6;
            line.endPose.yaw = optimizeRobRotation(line.endPose.yaw);
            g_Auto_Num++;

            break;
        case 21:
            if(StraigthLinePathInit(&line, false, 1000, 1000, 400, 0, 25, 20));
            g_Auto_Num++;
            break;
        case 22:
            if (goPath() == FINISH) {

//                setPidCalMode(PID_OFF);
                g_Auto_Num++;
            }
            break;
            /////路径六结束
        case 23:
            /****左放苗2开始****/

            chassis[5]=1;

            /****左放苗2结束****/
//            g_Auto_Num++;
//            Motor_ClearPID();
            break;
            /////路径七开始              //第三次取苗
        case 24:
            Control_Point.Path_BesselPoint_1 = Robot_Pose;
//            static Pose gopath6 = { (-2118.6f), (2064.3f),-270.8f};  //直线路径6

            Control_Point.Path_BesselPoint_2.x = -1565.f;//-2118.6+553.6
            Control_Point.Path_BesselPoint_2.y = 1957.f;//2064.3-107.3

            Control_Point.Path_BesselPoint_3.x = -2090.f;//-1996.82-93.18
            Control_Point.Path_BesselPoint_3.y = 889.f;//180.f+709

            Control_Point.Path_BesselPoint_4.wx = -1996.82f;
            Control_Point.Path_BesselPoint_4.wy = 175.f;
            Control_Point.Path_BesselPoint_4.yaw = -361.87f;        //TODO

            g_Auto_Num++;
            break;
        case 25:

            if(Path_BesselPathInit(&Control_Point, false, 3000, 3000, 350, 5, 50, 60));

            g_Auto_Num++;

            break;
        case 26:
            if (goPath() == FINISH) {
                setPidCalMode(PID_OFF);
                g_Auto_Num++;
            }
            break;
            /////路径七结束
        case 27:

            /****右取苗开始****/

            chassis[0]=1;

            /****右取苗结束****/
//            g_Auto_Num++;
//            Motor_ClearPID();
            break;
            /////路径八开始      //第五次放苗
        case 28:

            Control_Point.Path_BesselPoint_1 = Robot_Pose;

            Control_Point.Path_BesselPoint_2.x = -1853.82f;
            Control_Point.Path_BesselPoint_2.y = 1007.f;

            Control_Point.Path_BesselPoint_3.x = -1767.83f;
            Control_Point.Path_BesselPoint_3.y = 1545.18f;

            Control_Point.Path_BesselPoint_4.wx = -1635.83f;
            Control_Point.Path_BesselPoint_4.wy = 2009.4f;
            Control_Point.Path_BesselPoint_4.yaw = -451.f;

            g_Auto_Num++;

            break;
        case 29:
            if(Path_BesselPathInit(&Control_Point, false, 2000, 2500, 400, 5, 40, 70));

            g_Auto_Num++;

            break;
        case 30:
            if (goPath() == FINISH) {
                setPidCalMode(PID_OFF);

                g_Auto_Num++;
            }
            break;
            /////路径八结束
        case 31:
            /****右放苗2开始****/

            chassis[2]=1;

            /****右取苗2结束****/
//            g_Auto_Num++;
//            Motor_ClearPID();
            break;
            /////路径九开始                  // 第六次放苗
        case 32:
            line.startPose = Robot_Pose;
            line.endPose = gopath9;
            line.endPose.yaw = optimizeRobRotation(line.endPose.yaw);

            g_Auto_Num++;

            break;
        case 33:
            if(StraigthLinePathInit(&line, false, 1200, 1200, 0, 0, 30, 20));

            g_Auto_Num++;

            break;
        case 34:
            if (goPath() == FINISH) {

                setPidCalMode(PID_OFF);
                g_Auto_Num++;
            }
            break;
            /////路径九结束
        case 35:
            /****右放苗1开始****/

            chassis[3]=1;

            /****右放苗1结束****/
//            Motor_ClearPID();
//            g_Auto_Num++;
            break;
            /////路径十开始          //第四次取苗
        case 36:
            Control_Point.Path_BesselPoint_1 = Robot_Pose;
//            static Pose gopath9 = { (-1176.25f), (2278.85f),-449.6f};  //直线路径9

            Control_Point.Path_BesselPoint_2.x = -622.65f;
            Control_Point.Path_BesselPoint_2.y = 2171.55f;

            Control_Point.Path_BesselPoint_3.x = -1398.04f;
            Control_Point.Path_BesselPoint_3.y = 851.f;

            Control_Point.Path_BesselPoint_4.wx = -1304.86f;
            Control_Point.Path_BesselPoint_4.wy = 136.f;
            Control_Point.Path_BesselPoint_4.yaw = -541.23f;

            g_Auto_Num++;

            break;
        case 37:
            if(Path_BesselPathInit(&Control_Point, false, 3000, 3000, 300, 5, 50, 60));

            g_Auto_Num++;

            break;
        case 38:
            if (goPath() == FINISH) {

//                setPidCalMode(PID_OFF);
                g_Auto_Num++;
            }
            break;
            /////路径十结束
        case 39:
            /****左取苗开始****/

            chassis[1]=1;

            /****左取苗结束****/
//            g_Auto_Num++;
//            Motor_ClearPID();
            break;
            /////路径十一开始                 //第七次放苗
        case 40:
            Control_Point.Path_BesselPoint_1 = Robot_Pose;

            Control_Point.Path_BesselPoint_2.x = -1161.86f;
            Control_Point.Path_BesselPoint_2.y = 969.f;

            Control_Point.Path_BesselPoint_3.x = -1312.43f;
            Control_Point.Path_BesselPoint_3.y = 1303.31f;

            Control_Point.Path_BesselPoint_4.wx = -1175.43f;
            Control_Point.Path_BesselPoint_4.wy = 1787.53f;
            Control_Point.Path_BesselPoint_4.yaw = -630.f;

            g_Auto_Num++;

            break;
        case 41:
            if(Path_BesselPathInit(&Control_Point, false, 2000, 2500, 400, 5, 40, 70));
            g_Auto_Num++;

            break;
        case 42:
            if (goPath() == FINISH) {

                setPidCalMode(PID_OFF);
                g_Auto_Num++;
            }
            break;
            /////路径十一结束
        case 43:
            /****左放苗1开始****/

            chassis[4]=1;

            /****左放苗1结束****/
//            g_Auto_Num++;
            break;
            /////路径十二开始                     //第八次放苗
        case 44:

            line.startPose = Robot_Pose;
            line.endPose = gopath12;
            line.endPose.yaw = optimizeRobRotation(line.endPose.yaw);
            g_Auto_Num++;

            break;
        case 45:
            if(StraigthLinePathInit(&line, false, 1200, 1200, 400, 0, 25, 20));
            g_Auto_Num++;
            break;
        case 46:
            if (goPath() == FINISH) {

//                setPidCalMode(PID_OFF);
                g_Auto_Num++;
            }
            break;
            /////路径十二结束
        case 47:
            /****左放苗2开始****/

            chassis[5]=1;

            /****左放苗2结束****/
//            g_Auto_Num++;
            break;
            /////路径十三开始             //平移到坡前
        case 48:

            line.startPose = Robot_Pose;
            line.endPose = gopath13;
            line.endPose.yaw = optimizeRobRotation(line.endPose.yaw);
            g_Auto_Num++;

            break;
        case 49:
            if(StraigthLinePathInit(&line, false, 1500, 2500, 1000, 5, 30, 60));
            g_Auto_Num++;
            break;
        case 50:
            if (goPath() == FINISH) {

//                setPidCalMode(PID_OFF);
                g_Auto_Num++;

            }
            break;
        case 51:
            setAirCylinder_more(Air_1,Close_Air,5,OUT_3,OUT_4,OUT_5,OUT_6,OUT_7);
            g_Auto_Num++;

            break;
            /////路径十三结束
            /////路径十四开始             //开始上坡
        case 52:

            line.startPose = Robot_Pose;
            line.endPose = gopath14;
            line.endPose.yaw = optimizeRobRotation(line.endPose.yaw);
            g_Auto_Num++;

            break;
        case 53:
            if(StraigthLinePathInit(&line, false, 2000, 3000, 800, 10, 25, 20));
            g_Auto_Num++;
            break;
        case 54:
            if (goPath() == FINISH) {
                setPidCalMode(PID_OFF);
                g_Auto_Num++;
            }
            break;
        case 55:
            Only_Once = 1;
            Hand_Shank_Data.Flag_Start_HandShank = true;
            if(Only_Once == 1)
            {
//                Motor_Set_param.vesc_shoot=10000;
                Only_Once++;}
//            Motor_ClearPID();
            break;
    }
}
